#include "pmsm.h"
#include "stdio.h"
#include "tim.h"


void motor_drv_init(void)
{
    HAL_TIM_PWM_Start(&SVPWM_TIMER, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&SVPWM_TIMER, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&SVPWM_TIMER, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&SVPWM_TIMER, TIM_CHANNEL_4);
    HAL_TIMEx_PWMN_Start(&SVPWM_TIMER, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&SVPWM_TIMER, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&SVPWM_TIMER, TIM_CHANNEL_3);
    HAL_TIM_Base_Start_IT(&SVPWM_TIMER);
}

void motor_param_init(p_motor motor)
{
    motor->status.enable = MOTOR_DIS;
    motor->status.tar_id = 0.0f;
    motor->status.tar_iq = 0.0f;
    motor->status.tar_speed = 0.0f;
    motor->status.tar_toq = 0.0f;

    motor->encoder.pole_pair = POLE_PAIR;
    motor->encoder.e_angle = 0.0f;
    motor->encoder.m_angle = 0.0f;
    motor->encoder.radian = 0.0f;
    motor->encoder.count = 0;
    // motor->encoder.count_last = 0;
    motor->encoder.z_flag = 0;
    motor->encoder.offset = 0;

    motor->svpwm.udc = MOTOR_DRV_UDC;
    motor->svpwm.ts = MOTOR_TS;   //100 uS, pwm value max is 2100
    motor->svpwm.max_ts = MOTOR_MAX;//80
    motor->svpwm.ialpha = 0;
    motor->svpwm.ibeta = 0;
    motor->svpwm.adc_ts = ADC_TS;
    motor->func.set_ch_pwm = set_ch_pwm;
    motor->status.enable = MOTOR_EN;
    motor_drv_init();

}


void motorTest_encoderCloseLoop(p_motor motor)
{
    
    motor->encoder.m_angle = (float)(TIM2->CNT)/(float)ENCODER_COUNT * 360.0f;  // mechanical angle, 16384 pulse per circle
    motor->encoder.radian = motor->encoder.m_angle * (float)POLE_PAIR * ECODER_RADIAN_COEF; //ploses is 5,  pi/180 = 0.01745

    // static float radian_temp = 0.0f; // give a increased radian
    // radian_temp += 0.0002f;
    // if(radian_temp > 3.1415926 * 2.0f)
    // {
    //     radian_temp = 0.0f;
    // }
    // motor->encoder.radian = radian_temp;
    
    motor->status.enable = MOTOR_EN;
    motor->svpwm.ts = PWM_CENT_VALUE * 2.0f;
    motor->svpwm.udc = MOTOR_DRV_UDC;
    motor->svpwm.adc_ts = 100;

    // clark_trans(motor);
    // park_trans(motor);
    // pid_ctrl(&motor->id_pid, motor->svpwm.id, motor->id_pid.tar);
    // pid_ctrl(&motor->iq_pid, motor->svpwm.iq, motor->iq_pid.tar);
    static float tar_id = 0.001f;
    static float tar_iq = 0.5f;
    anti_park(motor, tar_id, tar_iq);
    sector_judgment(motor);
    pwm_duration_calc(motor);
    //safty function()
    pwm_out(motor);
}

void foc_printf(p_motor motor)
{
    /*
    // printf("%0.3f %4d \
    //         %0.2f %0.2f %0.2f \
    //         %0.2f %0.2f \
    //         %0.2f %0.2f \
    //         %0.3f %0.3f \
    //         %0.3f %0.3f %0.3f %0.3f \
    //         %4d %4d %4d %4d\r\n", \
    //         (float)motor->encoder.radian, (int)motor->svpwm.sector,\
    //         (float)motor->svpwm.ia, (float)motor->svpwm.ib, (float)motor->svpwm.ic,\
    //         (float)motor->svpwm.ialpha, (float)motor->svpwm.ibeta,\
    //         (float)motor->svpwm.id, (float)motor->svpwm.iq,\
    //         (float)motor->id_pid.out, (float)motor->iq_pid.out,\
    //         motor->svpwm.ta, motor->svpwm.tb, motor->svpwm.tc, motor->svpwm.td,\
    //         (int)TIM8->CCR1, (int)TIM8->CCR2, (int)TIM8->CCR3, (int)TIM8->CCR4);
    */
        printf("%0.3f %4d \
            %0.3f %0.3f %0.3f %0.3f \
            %4d %4d %4d %4d\r\n", \
            (float)motor->encoder.radian, (int)motor->svpwm.sector,\
            motor->svpwm.ta, motor->svpwm.tb, motor->svpwm.tc, motor->svpwm.td,\
            (int)TIM1->CCR1, (int)TIM1->CCR2, (int)TIM1->CCR3, (int)TIM1->CCR4);

}           


